// -*- C++ -*-
/*!
 * @file  TestOutState.cpp
 * @brief 
 *
 */
#include "TestOutState.h"

// Module specification
// <rtc-template block="module_spec">
static const char* testout_spec[] =
{
    "implementation_id", "TestOutState",
    "type_name",         "TestOutState",
    "description",       "TestOutState component",
    "version",           "1.0",
    "vendor",            "leus, JSK",
    "category",          "vb_utility",
    "activity_type",     "TestOutStateComponent",
    "max_instance",      "10",
    "language",          "C++",
    "lang_type",         "compile",
    ""
};
// </rtc-template>
TestOutState::TestOutState(RTC::Manager* manager)
    : RTC::DataFlowComponentBase(manager),
      m_outStatePort("state", m_outState),
      m_stateServicePort("StateService")
      // <rtc-template block="initializer">
      // </rtc-template>
{
}

TestOutState::~TestOutState()
{
}

RTC::ReturnCode_t TestOutState::onInitialize()
{
    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers
    
    // Set OutPort buffer
    addOutPort("state", m_outStatePort);
    std::string portname = std::string(getInstanceName()) + std::string(".") + std::string("state");
    m_outStateManager.registerStateServiceToOutPort(m_portAdmin.getPort(portname.c_str()));

    // Set service provider to Ports
    m_stateServicePort.registerProvider("stateservice", "StateService", m_stateServiceManager);
    // Set service consumers to Ports
  
    // Set CORBA Service Ports
    addPort(m_stateServicePort);
        
    // </rtc-template>

    // Bind variables and configuration variable
    bindParameter("componentconf_file", m_componentconf_file, "TestOutState.conf");
    bindParameter("config", m_config, "TestOutState0");
    
    return RTC::RTC_OK;
}


/*
  RTC::ReturnCode_t TestOutState::onFinalize()
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t TestOutState::onStartup(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t TestOutState::onShutdown(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/


RTC::ReturnCode_t TestOutState::onActivated(RTC::UniqueId ec_id)
{
    // 設定ファイルの読み込み
    coil::Properties fprop;    
    std::ifstream compf(m_componentconf_file.c_str());
    if (compf.fail()) 
    {
        std::cerr << "failed to open component configuration file: "<< m_componentconf_file << std::endl;
        return RTC::RTC_ERROR;
    }
    fprop.load(compf);

    // ステート作成開始
    // 全部クリア
    m_outStateManager.clear();

    // 設定ファイルから自分に関係する部分を探す
    // なければエラー
    if (fprop.findNode(m_config)==0)
    {
        std::cerr << "faild to find config node" << std::endl;
        return RTC::RTC_ERROR;
    }
    
    coil::Properties prop;
    prop << fprop.getNode(m_config);
     
    std::vector<coil::Properties *> props;
        
    // stateの追加
    if (prop.findNode("state") !=0)
    {
        coil::Properties p;
        p << prop.getNode("state");
        
        std::cerr << std::setw(40) << std::setfill('=') << " " << std::endl;
        props = p.getLeaf();
        for(unsigned int i = 0; i < props.size(); i++)
        {
            try {
                StatePtr state = StateBase::create(*props[i]);
                m_outStateManager.addState(state);
                std::cerr << "state:" << state->name() << std::endl;                
                m_stateServiceManager.addState(state);
            } catch(std::runtime_error &e) {
                std::cerr << e.what() << ":: ignored" << std::endl;
            }
        }
    }

    std::cerr << std::setw(40) << std::setfill('=') << " " << std::endl;
    
    m_timer.start();
    return RTC::RTC_OK;
}


RTC::ReturnCode_t TestOutState::onDeactivated(RTC::UniqueId ec_id)
{
    return RTC::RTC_OK;
}


RTC::ReturnCode_t TestOutState::onExecute(RTC::UniqueId ec_id)
{
    try
    {
        m_outStateManager >>= m_outState.dataset;
    }
    catch(const std::runtime_error &e)
    {
        // 失敗しても続行
        std::cerr << e.what() << std::endl;
    }

#ifdef DEBUG    
    std::cerr << "Lap time   : " << m_timer.tick()<< std::endl;
    std::cerr << "Total time : " << m_timer.time()<< std::endl;
    std::cerr << "\r\x1b[4A";    
#endif
    
    m_outStatePort.write();
    return RTC::RTC_OK;
}

std::string TestOutState::servicetest(const std::string &argv)
{
    std::cerr << "Input from serviceport is " << argv << std::endl;
    return ":serviceinput ok";
}

/*
  RTC::ReturnCode_t TestOutState::onAborting(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t TestOutState::onError(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t TestOutState::onReset(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t TestOutState::onStateUpdate(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/

/*
  RTC::ReturnCode_t TestOutState::onRateChanged(RTC::UniqueId ec_id)
  {
  return RTC::RTC_OK;
  }
*/



extern "C"
{
 
    void TestOutStateInit(RTC::Manager* manager)
    {
        coil::Properties profile(testout_spec);
        manager->registerFactory(profile,
                                 RTC::Create<TestOutState>,
                                 RTC::Delete<TestOutState>);
    }
  
};
